#include "Arduino.h"
#include "motor.h"

Motor::Motor(int motor1_Red, int motor1_Black, int motor2_Red, int motor2_Black){
    int i;
	
	motor_left[0] = motor1_Red;
	motor_left[1] = motor1_Black;
			
	motor_left[0] = motor2_Red;
	motor_left[1] = motor2_Black;
	
	
    for(i = 0; i < 2; i++){
        pinMode(motor_left[i], OUTPUT);
        pinMode(motor_right[i], OUTPUT);
    }
}

void Motor::stop(){
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], LOW);

    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], LOW);
    delay(25);
}

void Motor::drive_forward(){
    digitalWrite(motor_left[0], HIGH);
    digitalWrite(motor_left[1], LOW);

    digitalWrite(motor_right[0], HIGH);
    digitalWrite(motor_right[1], LOW);
}

void Motor::drive_backward(){
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], HIGH);

    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], HIGH);
}

void Motor::turn_left(){
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], HIGH);

    digitalWrite(motor_right[0], HIGH);
    digitalWrite(motor_right[1], LOW);
}

void Motor::turn_right(){
    digitalWrite(motor_left[0], HIGH);
    digitalWrite(motor_left[1], LOW);

    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], HIGH);
}